﻿#pragma once

#include<math.h>
#include<iostream>
#include <algorithm>
#include <optional>
#include <iomanip>

#include "dataStructAlg.h"

#include "../src_slide/include/GoldenFinger.h"
#include "../src_slide/include/PairWires.h"
#include "Wire.h"
#include "../src_slide/include/Polygon.h"
#include <fstream>
#include <boost/geometry.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/preprocessor/control/iif.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <unordered_set>


// Utils中的函数
double norm(Point a);
double norm(const Line& a);
double cross(Point a, Point b);											// 叉乘

double getTwoLinesDist(Line line1, Line line2);
double backMinDistNum(std::vector<double> DistNums);
double pointToSegmentDistance(Point basePoint, Line baseLine);			// 计算一个点到一条线段的最短距离
Point getProjectionPoint(Point basePoint, Line baseLine);				// 求出投影点的位置
Point RotateVec(Point vec, double angle);								// 返回向量旋转后的角度 || > 0 --> 逆时针
Point getInsection(Point Vec1, Point p1, Point Vec2, Point p2);			// 已知两条线段的方向向量和线段上一点求两条直线的交点，求交点
Point rotate(Point vec, double angle);                                  // 向量旋转,[1] > 0:逆时针
bool crossSegs(Line line1, Line line2);									// 优化判断：两条线段是否有交点
bool judgeCrossPoint(Point point, Line line);							// 判断点是否在线段中间
double CrossVecs(Point a, Point b);                                     // 两个向量的叉乘
void NormalizeVec(Point& NormalVec);                                    // 向量单位化
double pointToLineDist(Point point, Line line1);						// 点到直线的距离
double linearDist(Point point1, Point point2);							// 得到两点之间的直线距离
Point getLineNearestPoint(Point baseP, Line baseL);						// 在baseL中找到和baseP最近的端点
bool checkPointInSegment(Point tP, Line segmentL);						// 检查一个点是否在一条线段上面
void exchangePoints(Point& point1, Point& point2);						// 交换两点（向量）的坐标并返回
int direction(Line& line, Point& point);								// 用来判断推挤线是否超过相邻线段的水平位置
bool isParallel_lines(Line line1, Line line2);							// 两条线是否是平行线
bool CheckLinesPerpendicular(Line firstSeg, Line SecondSeg);			// 判断
bool inspectVectorsDirection(Point vec1, Point vec2);					//判断两个向量方向一致否
void removeLine(Line line, std::vector<Line>& lines);					//一组线中删除某条线
int getPosition(const std::vector<Line>& lines, Line& line);					//获取当前线段在线组中的位置
void mergeLines(std::vector<Line>& lines);								//合并线段
bool pointInPolygon(Point p, const std::vector<std::vector<Line>>& obss, bool polygonIncludeOutline);		//判断点是否在障碍物内部，用射线法，交点为奇数说明在障碍物内（交点在多边形端点只算一次,增加分割点）


namespace v2Hash {
	struct PointHash {
		size_t operator()(const Point& pnt)const;
	};
	struct LineHash {
		size_t operator()(const Line& line)const;
	};
}


// Hug
struct PointHash {
	size_t operator()(const Point& data) const {
		return std::hash<double>()(data.x) ^ std::hash<double>()(data.y); //^ std::hash<double>()(data.Pt2.x);
	}
};

bool CheckSegmentAngle(const Line& InputSegment);								 // 判断走线角度是否符合 135°要求
Point CountLineMidPos(const Point& Point1, const Point& Point2);                 // 返回两个点连线的中点
Point CountLineMidPos(const Line& BaseSegment);									 // 返回一条线段的中点
double CountLineAngle(const Line& BaseSeg);										 // 返回一条线的角度（0° - 180°）
Point CountCrossingPoint(Point Vec1, Point p1, Point Vec2, Point p2);            // 给定两点以及该点所在的某个方向求交点
std::vector<Line> PointsToLines(std::vector<Point> points, bool flag);			 // 将一些点按照顺序连接成线
std::vector<Point> LinesToPoints(std::vector<Line> lines, bool flag);			 // 线->点
Point getCenterOfObstacles(const std::vector<std::vector<Line> >& InputObstacles);                 // 获取一组障碍物的中心点
void DeleteSameSegment(std::vector<Line>& InputLinesVec);                                          // 一组线中删除相邻相同的线段
bool CheckNumInSets(const double& InputNumber, const std::vector<double>& InputNumSets);           // 判断InputNumber是否在InputNumSets数字集合中
bool CheckNumInSets(const int& InputNumber, const std::vector<int>& InputNumSets);				   // 判断InputNumber是否在InputNumSets数字集合中
std::vector<std::vector<Line> > ExpandWire(const std::vector<Line>& InputWireLines, const double& ExpandDistance);                             // 对一整线线进行外扩   
std::vector<Line> ExpandSegment(const Line& InputSegment, const double& ExpandDistance);            // 对一条线段进行膨胀
void sortLines(std::vector<Line>& lines);                                                           // 对一组线进行首尾相连排列 Pt1 -> Pt2
Line getSideLine(Line line, std::vector<Line> lines);                                               // 一组线中找到和line相连接的线
int getPointPosition(const std::vector<Point>& pointsVec, const Point& basePoint);			 // 获取当前点在点集中的位置
bool isEncloseGraph(std::vector<Line> lines);												 // 判断输入的一组线是障碍物还是线
bool pointInPointsVec(Point point, std::vector<Point> points);								 // 判断一个点是否在一个点集中
void pointsToNewVec(std::vector<Point> points_, std::vector<Point>& newPointsVec);           // 将Points中的点全部添加到newPointsVec中去
std::vector<Line> linkToLine(Point sp_, Point ep_);											 // 将两点直接连成线
Point getMidpoint(Point point1, Point point2);                                               // 求中点
Point getMidpoint(const Line& line);
void removePoint(Point point, std::vector<Point>& points);									 // 去除点集中的某个点
int getPointPositionInLines(Point point, std::vector<Line> lines);							 // 点在哪一条线上
Line getShoveSegment(std::vector<Line> shoveSingleLines, Line slideLine);					 // 找到会被推挤的线段
Line getShoveSegment(const Wire& shoveSingleLines, const Line& slideLine);				     // 找到会被推挤的线段
bool determineLineInSlidePath(Line parallelLine, Line slideLine);							 // 确定和SlideLine平行的平行线段是否在slide的路径上即会不会被推挤到
void exchangeTwoLinesVec(std::vector<Line>& linesLeft, std::vector<Line>& linesRight);       // 交换两个线组
void linesExpand(std::vector<Line>& lineVec, double distance);								 // 线的外扩
void extendEndingPoint(std::vector<Line>& singleLinesVec, const double& dist);				 // 一根线两端点的延长
void inspectSegmentsConnection(std::vector<Line>& singleLinesVec);							 // 对整根线的检查
void sortSingleLines(std::vector<Line>& singleLinesVec);									 // 对整根线进行排序
void RemovePointSegment(std::vector<Line>& InputCheckSegs);									 // 去除线组中起点和终点相同的线段
bool intersectionTwoLines(std::vector<Line> lines_1, std::vector<Line> lines_2);			 // 两根线的交点
std::vector<Point> insectionsOfLineToLines(Line line, std::vector<Line> lines);				 // 求一条线段和多条线段的交点
void sortPoints(Line line, std::vector<Point>& points);										 // 将最后的点从起点到终点依次排好
double PointsDist(Point p1, Point p2);														 // 计算两个点之间的曼哈顿距离
bool checkSamePointsVec(const std::vector<Point>& pointsVec_large, const std::vector<Point>& pointsVec_small);      // 判断pointsVec_small中是不是只有顶点没有其他点了
void exchangeLine(Line& line1, Line& line2);																		// 交换两条线段
void exchangeTwoNum(int& num1, int& num2);																			// 交换两个数据
bool isSegPolygonCross(const Line& inputLine, const std::vector<Line>& polygon, const bool& limitCrossing);			// 判断线段与多边形是否相交
double countSegmentsLength(const std::vector<Line>& lines);															// 计算一组线段的总长度
double getCosineValue(double oppoSide, double nextLeftSide, double nextRightSide);								    // 余弦定理求余弦值
std::vector<Line> WireExpandBoost(const Wire& InputWireSegs, const double& ExpandDistance);                         // 使用Boost的Buffer实现对线的外扩
double getParallelLineDist(const Line& OneSeg, const Line& SecondSeg);												// 两条平行线段的间距 
bool BOOSTinPolygon(const Point& pt, const std::vector<Line>& poly);												// 判断点是否在多边形内部
bool isClockwise(const std::vector<Point>& points);
std::vector<Point> getTwoLinesCrossingPoints(const std::vector<Line>& lines_1, const std::vector<Line>& lines_2, const bool& ConsiderEndPoints);		   // 计算两条线的交点
bool LiveWireGetNewSegment(const Point& StartP_, const Point& EndP_, const std::vector<Line>& InputWireLines, std::vector<Line>& PrintOutHugLines);        // 从一根线上取 HugSegments
bool getSegmentsToLinesCrossingPoints(const Line& InputSegment, const std::vector<std::vector<Line> >& InputLinesVec, const bool& ConsiderEndPoints);      // InputSegment 和每一个障碍物的交点是否都是偶数
bool RetrunTrueHugSegments(const std::vector<Line>& PreSlideWires, const std::vector<Line>& AfterSlideWires, std::vector<Line>& RealHugSegments);          //Return the line segments in line group a that are not in line group b

bool linesExpandToPoly(const std::vector<Line>& linesVec, double distance, std::vector<Line>& ExpandResultPoly);
void addPoint(std::vector<Line>& line, Point pt);
Line lineReverse(Line l);
bool isInWhichParallelSegs(const Line& InputSlideSeg, const std::vector<Line>& InputParallelSegs, const std::vector<int>& NumIndex);
bool BoostGetCrossingPoints(const Point& InputPointOne, const Point& InputVecOne, const Point& InputPointTwo, const Point& InputVecTwo, Point& CrossingPoint);  // 使用Boost库计算两条直线的交点
bool BoostGetCrossingPoints(const Line& InputSegOne, const Line& InputSegTwo, Point& CrossingPoint);
bool CheckWireLinesCross(std::vector<Line>& InputSingleLinesVec);				// Hug后的线组是否出现线段交叉
int GetPointToSideMinDist(const Point& InputPoint, const std::vector<Line>& PolySegments);       // 点到一组线段中的哪条线段最近
bool sortBusSlideDir(std::vector<int>& inputId, const std::vector<Line>& BaseSegs, const Point& SlideDir, const bool& SortDir);   // Hug中对Bus按SlideDir进行排序 从SlideDir方向从前向后
Point getSlideProjectionUtils(const Point& offsetVec, const Point& baseVec);
void reversePolygon(std::vector<Line>& poly);
bool getSegAndPolyInsectionsBoost(const Line& inputSegment, const std::vector<Line>& inputPolygon, std::vector<Point>& CrossingPoints);  // 计算一条线段和一个多边形的交点
bool CheckSegsCrossBoost(const Line& SegOne, const Line& SegTwo);  // 判断两条线段是否相交
bool CheckSegCrossPBoost(const Line& input, const std::vector<Line>& inputPolygon);  // 判断线段和多边形是否相交
bool CheckRoutePathSame(const std::vector<Line>& inputPathOne, const std::vector<Line>& inputPathTwo);   // inputPathOne 的线段是否存在inputPathTwo中







